/*
   Das Programm "Receiver" empfaengt Telegramme von dem Command&Control Center
   und stellt die Servos entsprechend..
 
 This sketch receives UDP message strings, prints them to the serial port
 and sends an "acknowledge" string back to the sender
 
 A Processing sketch is included at the end of file that can be used to send 
 and received messages for testing with a computer.
 
 created 21 Aug 2010
 by Michael Margolis
 
 This code is in the public domain.
 */


/*
Servo Steuerung

by Tom Igoe
additions by Carlyn Maw, Marc Tiedemann
translation by Marcus Paeschke
Created 28 Jan. 2006
Updated 7 Jun. 2006
Translated 9 May 2008

Die Werte fuer den kuerzesten Puls (minPulse) und den luengsten Puls
(maxPulse) kuennen je nach Modell des eingesetzten Servos variieren.
Im Idealfall sollten die Pulse zwischen einer und zwei Millisekunden
lang sein, in der Praxis haben sich aber auch Werte zwischen
500 und 2500 Mikrosekunden bewuehrt. Probiert selbst aus, welche
Werte am besten fuer euch funktionieren.

Dieser Code benutzt die Funktion millis() um die Zeit zwischen den
Pulsen zu bestimmen. Die Funktion erzeugt allerdings nach etwa 5 Tagen
Laufzeit einen Speicherueberlauf, d.h. die Zahl wird groesser als sie in
der Variable abgespeichert werden kann. Solltet ihr ein Programm
schreiben, was entsprechend lange laufen muss, denkt bitte daran.
 */

#include <Servo.h> 
#include <SPI.h>                 // needed for Arduino versions later than 0018
#include <Ethernet.h>
#include <EthernetUdp.h>         // UDP library from: bjoern@cs.stanford.edu 12/30/2008
#include <avr/wdt.h>

/*************************************************
 Definitionen 
 *************************************************/

// Steuerpins fuer die Servos: Pin 0 und 1 sind fuer RX/TX belegt.  
// Pins ab 2 sind verwendbar.
//define servoPinStart 26 
#define servoPinStart 26
//define servoPinende 40#
#define SERVO_MAX 4
#define UDP_TIMEOUT 1000  //in 1,3ms
#define forEachServo for (i=0; i<SERVO_MAX; i++)
//#define logln Serial.println
//#define log Serial.print

//Debug Info

#define DL_INFO 0x0001
#define DL_WARN 0x0002
#define DL_ERR  0x0004
#define DL_ADC  0x0008
#define DL_UDP_RCV  0x0010
#define DL_UDP_SENT 0x0020
#define DL_SERVO  0x0040
#define DL_SERIAL 0x4000
#define DL_UDP    0x8000
#define DL_ALL    0xFFFF
//#define DL_STARTUP DL_UDP|DL_SERVO|DL_SERIAL|DL_UDP|DL_UDP_SENT
#define DL_STARTUP DL_SERIAL|DL_UDP_SENT|DL_UDP_RCV

#define RL_NULL 0x00
#define RL_INIT 0x01
#define RL_UDP  0x02

/*************************************************
 Globale Variablen
 *************************************************/
Servo servo[SERVO_MAX];
short servoFailSave[SERVO_MAX]= {90,90,92,90};

// An EthernetUDP instance to let us send and receive packets over UDP
EthernetUDP Udp;
// Enter a MAC address and IP address for your controller below.
// The IP address will be dependent on your local network:
byte mac[] = {0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED};
IPAddress ip(192, 168, 1, 177);
IPAddress dns(192, 168, 1, 1);
IPAddress subnet(255, 255, 255, 0);
IPAddress gateway(192, 168, 1, 1);
unsigned int localPort = 8888; // local port to listen on
//float KorrekturfaktorAnalog = 4.615; //Referenz Pin 3.3Volt
float KorrekturfaktorAnalog = 4.615*5.92; //Mit Spannungsteiler

// buffers for receiving and sending data
byte packetBuffer[UDP_TX_PACKET_MAX_SIZE]; //buffer to hold incoming packet,
//char ReplyBuffer[] = "acknowledged"; // a string to send back 
//byte ReplyBufferControl[8]; //string with timestamp to send back
short Spannungswerte[16]; //A0-A15
int debugLevel= DL_STARTUP;
int runLevel= RL_NULL;
String mB; //message Buffer
char internalMessageBuffer[100];
char* messageBuffer= &internalMessageBuffer[1];
/*int minPulse = 500; // Minimalposition des Servos
int maxPulse = 2550; // Maximalposition des Servos
int pulse = 0; // Aktueller Pulswert
int servoSpeed = 5; // Bewegungsgeschwindigkeit

int refreshTime = 1; // Kurze Erfrischungszeit dazwischen

boolean updown = true; // Gehts jetzt Hoch oder Runter*/

/*************************************************
 Logging
 *************************************************/

#define logln log

void log(char* message);

void log(String message)
{
  //Serial.println(message);
  internalMessageBuffer[0]='*';
  message.toCharArray((char*)messageBuffer, sizeof(internalMessageBuffer)-1);
  log((char*)messageBuffer);
}

void log(char* message)
{
    int size= strlen(message);
    //Serial.println(message);
    if (debugLevel & DL_SERIAL)
    {
        if (size > UDP_TX_PACKET_MAX_SIZE)
        {
          Serial.print("*");
        }
        Serial.println(message);
        size= UDP_TX_PACKET_MAX_SIZE;
    }

    if (debugLevel & DL_UDP)
    if (runLevel & RL_UDP)
    {
        Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
        Udp.write((byte*) message, size);
        Udp.endPacket();
    }
    else
    {
        Serial.println("UDP not set up\n");
    }
}
 
void adcDebug(int analogChannel, short u)
{
    if (debugLevel & DL_ADC)
    {
        mB="analog input ";
        mB.concat(analogChannel);
        mB.concat(" is ");
        mB.concat(u);
        mB.concat(" mV\n");
        log(mB);
    }
}

void debugServoSwitch(int i, short value, char* s_value)
{
    if (debugLevel & DL_SERVO)
    {
        mB="Switching Servo ";
        mB.concat (i);
        mB.concat ("    ");
        mB.concat (value);
        mB.concat (" Bytes: ");
        mB.concat (s_value[0]);
        mB.concat(s_value[1]);
        log(mB);
    }
}

void debugUDP(byte packetBuffer[], int packetSize) 
{
    if (debugLevel & DL_UDP_RCV)
    {
        mB="Received packet of size ";
        mB.concat(packetSize);
        mB.concat("\n");
        mB.concat("From ");
        IPAddress remote = Udp.remoteIP();
        for (int i = 0; i < 4; i++) {
            mB.concat(remote[i]);
            if (i < 3) {
                mB.concat(".");
            }
        }
        mB.concat(", port ");
        mB.concat(Udp.remotePort());
        mB.concat("\n");
        mB.concat("Contents:");
        mB.concat((char*) &packetBuffer[0]);
        log(mB);
    }
}

void debugReply(byte packetBuffer[], int packetSize)
{
    if (debugLevel & DL_UDP_SENT)
    {
        mB="Reply Sent: '";
        mB.concat(packetSize);
        mB.concat((char*)packetBuffer);
        mB.concat("'\n");
        log(mB);
    }
}

/*************************************************
 Setup & Initialization
 *************************************************/
//Low Level Servo Interface
//void servoFailSavePosition();

void setupServo() {
    int servoPin = servoPinStart;
    int i = 1;
    forEachServo
    {
        servo[i].attach(servoPin);
        servoPin = servoPin + 2;
        wdt_reset();
    }
}

void setupUdp() {
    // start the Ethernet and UDP:
    Ethernet.begin(mac, ip, dns, subnet, gateway);
    Udp.begin(localPort);
    runLevel|= RL_UDP;
}

void setupLed() {

    pinMode(46, OUTPUT);
    pinMode(48, OUTPUT);
    pinMode(50, OUTPUT);
    digitalWrite(46, HIGH);
    digitalWrite(48, HIGH);
    digitalWrite(50, HIGH);
}

//High Level Control - MAIN

void setup() {
    /*pinMode(servoPin, OUTPUT); // Servopin ist ein Ausgabepin
    pulse = minPulse; // Position des Servos startet auf Minimum*/
    Serial.begin(9600); // Serielle Verbindung zur Kontrolle
    mB= "Servoansteuerung startend auf Pin ";
    mB.concat(servoPinStart);
    log(mB);
    setupServo();
    setupUdp();
    setupLed();
    servoFailSavePosition();
    //wdt_enable(WDTO_2S);

}

/*************************************************
 Helper
 *************************************************/
void servoTest() {
    int i = 1;
    mB="Alle Servos drehen rechts!";
    log(mB);
    forEachServo
    {
        servo[i].write(170);
        wdt_reset();
    }

    delay(300);
    log("Alle Servos drehen links!\n");

    forEachServo
    {
        servo[i].write(20);
        wdt_reset();
    }
    delay(300);
}

void ledTest() {
    int i = 0;
    log("LED ON!\n");
    pinMode(44, OUTPUT);
    digitalWrite(44, HIGH);
    delay(300);

    log("LED OFF\n");
    digitalWrite(44, LOW);
    delay(300);
}

void hornTest() {
    int i = 0;
    log("HORN ON!\n");
    pinMode(46, OUTPUT);
    digitalWrite(46, HIGH);
    delay(350);

    log("HORN OFF\n");
    digitalWrite(46, LOW);
    delay(100);
}

void lighton() {
    log("LIGHT ON!\n");
    digitalWrite(46, HIGH);
    digitalWrite(48, HIGH);
    digitalWrite(50, HIGH);
}

void lightoff() {
    log("LIGHT OFF!\n");
    digitalWrite(46, LOW);
    digitalWrite(48, LOW);
    digitalWrite(50, LOW);
}

void servoFailSavePosition() {
    int i = 0;
    log("Alle Servos sind jetzt auf FailSave!\n");
    forEachServo
    {
        servo[i].write(servoFailSave[i]);
        wdt_reset();
    }
}

void adcUpdate() {
    for (int analogChannel = 0; analogChannel < 16; analogChannel++) {
	    Spannungswerte[analogChannel]= analogRead(analogChannel) * KorrekturfaktorAnalog;
        adcDebug(analogChannel, Spannungswerte[analogChannel]);
    }
}

/*************************************************
 Parser
 *************************************************/
void parser(char* command) {
    char mainCommand = command[0];
    //char subCommand= command[9];
    switch (mainCommand) 
    {
        case 'X': //servoFailSavePosition
        case 'x':
        {
            servoFailSavePosition();
        }
        break;

        case 'D':
        {
            mB= "DebugLevel set from '";
            mB.concat(debugLevel);
            debugLevel= command[10];
            debugLevel+= command[11]*0xFF;
            mB.concat("' to '");
            mB.concat(debugLevel);
            mB.concat("'\n");
            log(mB);
        }
        break;

        case 'R':
        case 'r':   //RunMode     
		{      
			int i= 0;
			short value= 90;
			forEachServo
			{
				char* s_value= (char*) &value;
				memcpy (s_value, &command[14+i*2], 2);
                                debugServoSwitch(i, value, s_value);
				servo[i].write(value);
				wdt_reset();	 
			}
		}
		break;
	
        case 'U': //Abfrage der Spannungen
        case 'u':
        {
            Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
            Udp.write((uint8_t*) (Spannungswerte), sizeof (Spannungswerte));
            Udp.endPacket();
        }
        break;

        case 'T': //ServoTest
        case 't':
        {
            int i = 0;
            logln("Testing Servo");
            for (i = 0; i < 3; i++) {
                servoTest();
                wdt_reset();
            }
        }
        break;

        case 'F': //LED Test: Turns on / off Led on Pin 44
        case 'f':
        {
            int i = 0;
            logln("Switching LED on Pin 44");
            for (i = 0; i < 3; i++) {
                ledTest();
                wdt_reset();
            }
        }
        break;

        case 'H': //HORN Test: Turns on / off Horn on Pin 45
        case 'h':
        {
            int i = 0;
            logln("Switching HORN on Pin 45");
            for (i = 0; i < 3; i++) {
                hornTest();
                wdt_reset();
            }
        }
        break;

        case 'L': //Licht ein
        case 'l':
        {
            logln("Switching Led/Light on Pin 48/50/52");
            {
                lighton();
                wdt_reset();
            }
        }
        break;

        case 'O': //Licht aus
        case 'o':
        {
            logln("Switching Led/Light on Pin 48/50/52");
            {
                lightoff();
                wdt_reset();
            }
        }
        break;

        default:
            logln("Invalid Command");
        break;
    }
}

/*************************************************
 UDP
 *************************************************/
 //Returns number of Bytes in the packet
 //Param: packetBuffer with the last received (!) packet
 //       First 9 Bytes are used for id (same as in received packet)
 //       Rest is used for payload (Voltages, Status)
 //       Max Size of packetBuffer is UDP_TX_PACKET_MAX_SIZE
int ReplyBufferControl_set(byte packetBuffer[])//Copy timestamp to ReplyBufferControl
{
    int payloadStart= 9;
    int payload= 6*sizeof(short);  //6 shorts statt 3 floats
    int packetSize= payloadStart+payload;
    // packetBuffer[0]:   indicates Type of Telegram (-> Parser)
	// packetBuffer[1..8]:TimeStamp
	// packetBuffer[9..]: optional UserData
    // Copy TimeStamp
    // memcpy(ReplyBufferControl, &packetBuffer[1], sizeof (ReplyBufferControl)); // Array with timestamp to send back
    memcpy(&packetBuffer[payloadStart], (char*)(&Spannungswerte[8]), payload); 
    packetBuffer[packetSize]= 0;
    return packetSize;
}

/*************************************************
 Main Thread (Endlosschleife)
 *************************************************/

void loop() {
    int i= 0; 
    int packetSize= 0;
    int packetFound= 0;
    int invalidPacketFound= 0;
    bool lastPacketFound= false;

    wdt_reset();
    adcUpdate();
    for(i=0; i < UDP_TIMEOUT; i++)
    {
        // if there's data available, read a packet
        packetSize = Udp.parsePacket();
        if (packetSize == 0)  //Kein Paket im Puffer
        {
            if (packetFound > 0)
            {
                //Wenn bereits ein Packet gelesen wurde und kein 
                //weiteres im Puffer ist, dann ist dieses Packet
                //das letzte.
                lastPacketFound= true;
                break;
            }
            delay(1);    
        }
        if (packetSize > 0) 
        {
            if (packetSize == UDP_TX_PACKET_MAX_SIZE)
            {
                // read the packet into packetBuffer
                Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);
                packetFound++;
                debugUDP(packetBuffer, packetSize);
            }
            else
            {
                Udp.read(packetBuffer, sizeof(packetBuffer));
                invalidPacketFound++;
                debugUDP(packetBuffer, packetSize);
            }
        }
    }
    if (invalidPacketFound > 0)
    {
        mB= "Invalid Packets: ";
        mB.concat(invalidPacketFound);
        log(mB);
    }
    
    if (lastPacketFound == false)
    {
        //Failsafe
        servoFailSavePosition();
        mB= "UDP_TIMEOUT -> FailSave";
        log(mB);
    }
    else
    {
        if (packetFound > 1)
        {
            mB= "Packets lost: ";
            mB.concat(packetFound-1);
            log(mB);
        }
        
 		//debugUDP(packetBuffer, packetSize);
        //ReplyBufferControl_set(); 
		
        parser((char*) packetBuffer);

        //send a reply, to the IP address and port that sent us the packet we received
		//packetBuffer is reused for received and sent data!
        packetSize= ReplyBufferControl_set(packetBuffer);
		Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
        Udp.write(packetBuffer, packetSize);
        //Udp.write(ReplyBufferControl);
        //Udp.write((uint8_t*)(Spannungswerte), sizeof(Spannungswerte));
        Udp.endPacket();
        debugReply(packetBuffer, packetSize);
        
        //delete packetBuffer; funktioniert nicht
    }
    
}



/*
  Processing sketch to run with this example
 =====================================================
 
 // Processing UDP example to send and receive string data from Arduino 
 // press any key to send the "Hello Arduino" message
 
 
 import hypermedia.net.*;
 
 UDP udp;  // define the UDP object
 
 
 void setup() {
 udp = new UDP( this, 6000 );  // create a new datagram connection on port 6000
 //udp.logln( true ); 		// <-- printout the connection activity
 udp.listen( true );           // and wait for incoming message  
 }
 
 void draw()
 {
 }
 
 void keyPressed() {
 String ip       = "192.168.1.177";	// the remote IP address
 int port        = 8888;		// the destination port
 
 udp.send("Hello World", ip, port );   // the message to send
 
 }
 
 void receive( byte[] data ) { 			// <-- default handler
 //void receive( byte[] data, String ip, int port ) {	// <-- extended handler
 
 for(int i=0; i < data.length; i++) 
 print(char(data[i]));  
 println();   
 }
 */




